
#include "vi5300.h"
#include "vi5300Firmware.h"
#include "iic1.h"
#include "debug.h"
#include "flash.h"
#include <math.h>
#include <stdlib.h>

#define VI5300_DEVICE_ADDR 0x6C
#define VI5300_REG_MCU_CFG 0x00
#define VI5300_RET_INT_STATUS 0x03
#define VI5300_REG_SYS_CFG 0x01
#define VI5300_REG_PW_CTRL 0x07
#define VI5300_REG_CMD 0x0a
#define VI5300_REG_SIZE 0x0b
#define VI5300_REG_SCRATCH_PAD_BASE 0x0c

#define VI5300_WRITEFW_CMD 0x03
#define VI5300_USER_CFG_CMD 0x09
#define VI5300_START_RANG_CMD 0x0E

#define VI5300_CFG_SUBCMD 0x01
#define VI5300_OTPW_SUBCMD 0x02
#define VI5300_OTPR_SUBCMD 0x03

#define PILEUP_A (9231000)
#define PILEUP_B (4896)
#define PILEUP_C (1922)
#define PILEUP_D (10)

#define XSHUT_Pin1 0      // GPIO_PIN_0
#define XSHUT_Pin2 0      // GPIO_PIN_1
#define XSHUT_Pin3 0      // GPIO_PIN_4
#define XSHUT_GPIO_Port 0 // GPIOA

uint8_t gSalve;
uint8_t chip_reg = 0xff;

/*****添加的程序*****/
uint8_t Calibration_data[10] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

typedef struct
{
    uint8_t Cali_Flag;
    uint8_t xtalk_cal;
    int16_t offset_cal;
    int16_t RefTof;
} VI5300_CALIBRATION_DATA;
VI5300_CALIBRATION_DATA cali_data;
/*******************/

typedef struct
{
    int16_t millimeter;
    uint32_t peak;
    uint16_t noise;
    uint32_t confidence;

} VI5300_Dist_TypeDef;

typedef struct
{
    int8_t xtalk_cal;
    uint16_t xtalk_peak;

} VI5300_XTALK_Calib_Data;

typedef struct OFFSET_CALIB
{
    int16_t offset_cal;
    int16_t RefTof;

} VI5300_OFFSET_Calib_Data;

typedef enum
{
    VI5300_OK = 0x00,
    VI5300_RANGING = 0x01,
    VI5300_BUSY = 0x02,
    VI5300_BUS_BUSY = 0x03,
    VI5300_SLEEP = 0x04,
    VI5300_BOOTING = 0x05,
    VI5300_ERROR = 0x06
} VI5300_Status;

typedef enum
{
    VI5300_DATA_VALID = 0x00,
    VI5300_RAW_DATA = 0x01,
    VI5300_DATA_ERROR = 0x02
} VI5300_RangingDataStatus;
typedef enum NEXTACTION
{
    VI5300_GO_ON = 0x00,
    VI5300_STOP = 0x01
} VI5300_NextAction;

VI5300_XTALK_Calib_Data xtalk_cal;

VI5300_OFFSET_Calib_Data offset_data;
// 判断芯片是否存在
static uint8_t existenceFlag = 0;

VI5300_Dist_TypeDef distance;

void ReadCalibrationDataFromFile(VI5300_CALIBRATION_DATA *cali_data)
{
    union byte_int16
    {
        uint8_t byte[2];
        int16_t int16_data;
    } byte_to_int16;

    readFlashPage(FLASH_SECTOR63_ADRR, (uint32_t *)Calibration_data, 6);

    cali_data->Cali_Flag = Calibration_data[0];
    cali_data->xtalk_cal = Calibration_data[1];
    byte_to_int16.byte[0] = Calibration_data[2];
    byte_to_int16.byte[1] = Calibration_data[3];
    cali_data->offset_cal = byte_to_int16.int16_data;
    byte_to_int16.byte[0] = Calibration_data[4];
    byte_to_int16.byte[1] = Calibration_data[5];
    cali_data->RefTof = byte_to_int16.int16_data;
}

void StoreCalibrationDataToFile(VI5300_CALIBRATION_DATA cali_data)
{
    // uint8_t index=0;
    union byte_int16
    {
        uint8_t byte[2];
        int16_t int16_data;
    } byte_to_int16;

    Calibration_data[0] = cali_data.Cali_Flag;
    Calibration_data[1] = cali_data.xtalk_cal;
    byte_to_int16.int16_data = cali_data.offset_cal;
    Calibration_data[2] = byte_to_int16.byte[0];
    Calibration_data[3] = byte_to_int16.byte[1];
    byte_to_int16.int16_data = cali_data.RefTof;
    Calibration_data[4] = byte_to_int16.byte[0];
    Calibration_data[5] = byte_to_int16.byte[1];
    writeFlashPage(FLASH_SECTOR63_ADRR, (uint32_t *)Calibration_data, 6);
    //	HAL_Delay(5);
    //	Flash_Read(0x0801F400, Calibration_data,6);
    //	printf("flash %d,%d,%d,%d,%d,%d\r\n",Calibration_data[0],Calibration_data[1],Calibration_data[2],Calibration_data[3],Calibration_data[4],Calibration_data[5]);
}

void VI5300_Set_Digital_Clock_Dutycycle(void)
{
    uint8_t t_reg;
    t_reg = 0x0F;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x0E;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    if (chip_reg == 0x00)
    {
        Ddl_Delay1ms(1);
        t_reg = 0x24;
        i2c1Write(VI5300_DEVICE_ADDR, 0xE9, &t_reg, 1);
        t_reg = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0xEE, &t_reg, 1);
        t_reg = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0xF5, &t_reg, 1);
    }
    Ddl_Delay1ms(4); // 默认4ms ，延时不准可以增加
}

void VI5300_WriteFirmwarePostConfig(void)
{
    uint8_t t_reg;
    t_reg = 0x0c;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_SYS_CFG, &t_reg, 1);
    t_reg = 0x06;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_MCU_CFG, &t_reg, 1);
    t_reg = 0xA0;
    i2c1Write(VI5300_DEVICE_ADDR, 0x3B, &t_reg, 1);
    t_reg = 0x80;
    i2c1Write(VI5300_DEVICE_ADDR, 0x3B, &t_reg, 1);

    if (chip_reg == 0x00)
    {
        t_reg = 0x24;
        i2c1Write(VI5300_DEVICE_ADDR, 0xE9, &t_reg, 1);
        t_reg = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0xEE, &t_reg, 1);
        t_reg = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0xF5, &t_reg, 1);
    }

    t_reg = 0x07;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_MCU_CFG, &t_reg, 1);
    t_reg = 0x02;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x00;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
}

void VI5300_WriteFirmware32Byte(uint8_t len, uint8_t *data)
{
    uint8_t cmd_size[2];

    if (len > 32)
        return;

    cmd_size[0] = VI5300_WRITEFW_CMD;
    cmd_size[1] = len;

    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, cmd_size, 2);
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, data, len);
}

void VI5300_WriteFirmwarePreConfig(void)
{
    uint8_t t_reg = 0x01;
    VI5300_Set_Digital_Clock_Dutycycle(); // set rco config add 20210129
    t_reg = 0x08;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x0a;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x06;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_MCU_CFG, &t_reg, 1);
    i2c1Read(VI5300_DEVICE_ADDR, VI5300_REG_SYS_CFG, &t_reg, 1);
    t_reg |= 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_SYS_CFG, &t_reg, 1);

    if (chip_reg == 0x00)
    {
        t_reg = 0x30;
        i2c1Write(VI5300_DEVICE_ADDR, 0x38, &t_reg, 1);
        i2c1Write(VI5300_DEVICE_ADDR, 0x3A, &t_reg, 1);
    }

    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, &t_reg, 1);
    t_reg = 0x02;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_SIZE, &t_reg, 1);
    t_reg = 0x00;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, &t_reg, 1);
    t_reg = 0x00;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE + 0x01, &t_reg, 1);
}

void VI5300_DownloadFirmware(uint8_t *buf, uint16_t size)
{

    uint8_t page = 0;

    VI5300_WriteFirmwarePreConfig();
    while (size >= 32)
    {
        VI5300_WriteFirmware32Byte(32, buf + page * 32);
        size -= 32;
        page++;
    }

    if (size > 0)
    {
        VI5300_WriteFirmware32Byte(size, buf + page * 32);
    }

    VI5300_WriteFirmwarePostConfig();
}

void VI5300_Interrupt_Enable(void)
{
    uint8_t loop = 0;
    uint8_t enable = 0;

    do
    {
        i2c1Read(VI5300_DEVICE_ADDR, 0x04, &enable, 1);
        enable |= 0x01;
        i2c1Write(VI5300_DEVICE_ADDR, 0x04, &enable, 1);
        i2c1Read(VI5300_DEVICE_ADDR, 0x04, &enable, 1);
        loop++;
    } while ((loop < 5) && (!(enable & 0x01)));

    if (loop >= 5)
    {
        // 中断出错
        existenceFlag = 0;
    }
}

void VI5300_Integral_Counts_Write(uint32_t inte_counts)
{
    uint8_t buf[4] = {0};
    // 小端模式，从小到大保存
    buf[0] = (inte_counts) & 0xFF;
    buf[1] = (inte_counts >> 8) & 0xFF;
    buf[2] = (inte_counts >> 16) & 0xFF;

    VI5300_Set_Digital_Clock_Dutycycle();
    uint8_t t_reg;
    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x03;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    t_reg = buf[0];
    i2c1Write(VI5300_DEVICE_ADDR, 0x0F, &t_reg, 1);
    t_reg = buf[1];
    i2c1Write(VI5300_DEVICE_ADDR, 0x10, &t_reg, 1);
    t_reg = buf[2];
    i2c1Write(VI5300_DEVICE_ADDR, 0x11, &t_reg, 1);
    t_reg = 0x09;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
}

void VI5300_Delay_Count_Write(uint16_t delay_count)
{
    uint8_t buf[2] = {0};

    // 大端模式
    buf[0] = (delay_count >> 8) & 0xFF;
    buf[1] = (delay_count) & 0xFF;

    VI5300_Set_Digital_Clock_Dutycycle();

    uint8_t t_reg;
    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x02;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x04;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    // 大端模式
    t_reg = buf[0];
    i2c1Write(VI5300_DEVICE_ADDR, 0x0F, &t_reg, 1);
    t_reg = buf[1];
    i2c1Write(VI5300_DEVICE_ADDR, 0x10, &t_reg, 1);
    t_reg = 0x09;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
}

void VI5300_Set_Integralcounts_Frame(uint8_t fps, uint32_t intecoutns)
{
    uint32_t inte_time;
    uint32_t fps_time;
    uint32_t delay_time;
    uint16_t delay_counts;

    inte_time = intecoutns * 1463 / 10;
    fps_time = 1000000000 / fps;
    delay_time = fps_time - inte_time - 1600000;
    delay_counts = (uint16_t)(delay_time / 40900);

    VI5300_Integral_Counts_Write(intecoutns);
    VI5300_Delay_Count_Write(delay_counts);
}

void VI5300_Wait_For_CPU_Ready(void)
{
    uint8_t stat;
    int retry = 0;

    do
    {
        Ddl_Delay1ms(1); // delay 1ms
        i2c1Read(VI5300_DEVICE_ADDR, 0x02, &stat, 1);
    } while ((retry++ < 20) && (stat & 0x01));
    if (retry >= 20)
    {
        existenceFlag = 0;
    }
}

VI5300_Status VI5300_Get_Interrupt_State(uint8_t *status)
{
    return i2c1Read(VI5300_DEVICE_ADDR, VI5300_RET_INT_STATUS, status, 1);
}
VI5300_Status WriteCommand(uint8_t cmd)
{
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, &cmd, 1);
}

void VI5300_Start_Continuous_Measure(void)
{
    uint8_t stat = 0;

    i2c1Read(VI5300_DEVICE_ADDR, VI5300_RET_INT_STATUS, &stat, 1);

    VI5300_Wait_For_CPU_Ready();

    VI5300_Set_Digital_Clock_Dutycycle();

    uint8_t t_reg = 0x0F;
    i2c1Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, &t_reg, 1);
}

#ifdef VI5300_TEMP_CALIBRATION // vi5300校准***********
VI5300_Status VI5300_Temp_Enable(uint8_t enable)
{
    uint8_t t_reg;

    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x0E;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    t_reg = enable;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0F, &t_reg, 1);
    t_reg = 0x09;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
    Ddl_Delay1ms(5);

    //	WriteOneReg(0x0C, 0x01);
    //	WriteOneReg(0x0D, 0x01);
    //	WriteOneReg(0x0E, 0x0E);
    //	WriteOneReg(0x0F, enable);
    //	WriteOneReg(0x0A, 0x09);
}
VI5300_Status VI5300_Check_Temp_Enable(uint8_t *enable)
{
    uint8_t t_reg;
    uint8_t tmp = 0;
    t_reg = 0x00;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x0E;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    t_reg = 0x09;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
    Ddl_Delay1ms(5);
    tmp = 0x09;
    i2c1Read(VI5300_DEVICE_ADDR, 0x0c, &tmp, 1);
    *enable = tmp;
}

VI5300_Status VI5300_Config_RefTof_Parameter(int16_t refTof)
{
    uint8_t tmp_tof;
    uint8_t t_reg;

    VI5300_Wait_For_CPU_Ready();
    VI5300_Set_Digital_Clock_Dutycycle();

    t_reg = 0x01;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x02;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x17;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    tmp_tof = refTof >> 8;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0F, &tmp_tof, 1);
    tmp_tof = refTof & 0xFF;
    i2c1Write(VI5300_DEVICE_ADDR, 0x10, &tmp_tof, 1);
    t_reg = 0x09;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
}

VI5300_Status VI5300_Check_RefTof_Parameter(int16_t *refTof)
{
    uint8_t t_reg;
    uint8_t tmp = 0;

    VI5300_Wait_For_CPU_Ready();
    VI5300_Set_Digital_Clock_Dutycycle();

    t_reg = 0x00;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x02;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x17;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    t_reg = 0x09;
    i2c1Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);

    Ddl_Delay1ms(5);
    i2c1Read(VI5300_DEVICE_ADDR, 0x0c, &tmp, 1);
    *refTof = tmp << 8;
    i2c1Read(VI5300_DEVICE_ADDR, 0xd, &tmp, 1);
    *refTof |= tmp;
}

void VI5300_Start_RefTof_Calibration(VI5300_OFFSET_Calib_Data *OFFSETCalibData)
{
    uint8_t retry = 0;
    uint8_t stat = 0xff, buf[32];
    int16_t milimeter;
    uint32_t peak2, peak_tmp;
    uint32_t integral_times;
    int32_t bias;
    int16_t i_reftof = 0;
    int16_t total = 0;
    int cnt = 0;
#ifdef VI5300_OFFSET_CALIBRATION
    int16_t ioffset = 0;
#endif

    do
    {
        VI5300_Temp_Enable(0);
        Ddl_Delay1ms(2);
        VI5300_Check_Temp_Enable(&stat);
    } while ((stat != 0) && (retry++ < 5));

    Ddl_Delay1ms(5);
    VI5300_Start_Continuous_Measure();
    while (1)
    {
        Ddl_Delay1ms(30);
        VI5300_Get_Interrupt_State(&stat);
        if ((stat & 0x01) == 0x01)
        {
            i2c1Read(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, buf, 32);
            i_reftof += (short)((((short)buf[21]) << 8) | (((short)buf[20])));
            milimeter = (int16_t)((((int16_t)buf[13]) << 8) | (((int16_t)buf[12])));

            peak2 = (uint32_t)((((uint32_t)buf[11]) << 24) | (((uint32_t)buf[10]) << 16) | (((uint32_t)buf[9]) << 8) | ((uint32_t)buf[8]));

            integral_times = (uint32_t)((((uint32_t)buf[25]) << 24) | (((uint32_t)buf[24]) << 16) | (((uint32_t)buf[23]) << 8) | ((uint32_t)buf[22]));
            integral_times = integral_times & 0x00ffffff;
            if (peak2 > 65536)
            {
                peak_tmp = (peak2 * 256) / integral_times * 256;
            }
            else
            {
                peak_tmp = peak2 * 65536 / integral_times;
            }

            peak_tmp = peak_tmp >> 12;
            bias = (int32_t)(PILEUP_A / (PILEUP_B - peak_tmp * PILEUP_D) - PILEUP_C) / PILEUP_D;
            if (bias < 0)
            {
                bias = 0;
            }
            milimeter = milimeter + (int16_t)bias;
            total += milimeter;
            cnt++;
        }
        if (cnt >= 20)
            break;
    }
    retry = 0;
    VI5300_Stop_Continuous_Measure();
    OFFSETCalibData->RefTof = i_reftof / 20;
#ifdef VI5300_OFFSET_CALIBRATION
    ioffset = total / 20;
    OFFSETCalibData->offset_cal = ioffset - OFFSET_POSITION; // 此处100是指在10cm处做offset标定，在多远距离做标定减多少，单位mm
#endif
    stat = 0;
    do
    {
        VI5300_Temp_Enable(1);
        Ddl_Delay1ms(2);
        VI5300_Check_Temp_Enable(&stat);
    } while ((stat != 1) && (retry++ < 5));
}
void VI5300_Config_RefTof(int16_t reftof)
{
    int16_t R_reftof;
    uint8_t retry = 0;

    do
    {
        VI5300_Config_RefTof_Parameter(reftof);
        Ddl_Delay1ms(2);
        VI5300_Check_RefTof_Parameter(&R_reftof);
    } while ((R_reftof != reftof) && retry++ < 5);
    Ddl_Delay1ms(2);
}
#endif //******以上，vi5300校准***********
uint8_t VI5300_Stop_Continuous_Measure(void)
{
    VI5300_Status ret = VI5300_OK;

    ret = WriteCommand(0x1F);
    Ddl_Delay1ms(10);
    return ret;
}

uint8_t vi5300Init(void)
{
    uint8_t stat = 0;
    uint8_t retry = 0;
    uint8_t t_reg = 0xFF, t_reg1 = 0xF0, t_reg2, chip_reg3a;

    i2c1Read(VI5300_DEVICE_ADDR, 0x06, &t_reg2, 1);
    if (t_reg2 == 0xD8)
    {
        for (retry = 0; retry < 5; retry++)
        {
            i2c1Read(VI5300_DEVICE_ADDR, 0x38, &t_reg, 1);
            if (t_reg == 0x30)
            {
                i2c1Read(VI5300_DEVICE_ADDR, 0x3A, &chip_reg3a, 1);
                if (chip_reg3a == 0x30)
                {
                    chip_reg = 0x30; // 应该使用这个版本的3.1版本的tof芯片
                    break;
                }
            }
            else if (t_reg == 0x00)
            {
                i2c1Read(VI5300_DEVICE_ADDR, 0x3A, &chip_reg3a, 1);
                if (chip_reg3a == 0x00)
                {
                    chip_reg = 0x00;
                    break;
                }
            }
        }
        if (chip_reg == 0x00)
        {
            return 0;
        }

        Ddl_Delay1ms(500);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x00, &t_reg2, 1);
        t_reg2 = 0x0c;
        i2c1Write(VI5300_DEVICE_ADDR, 0x01, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x01;
        i2c1Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x21;
        i2c1Write(VI5300_DEVICE_ADDR, 0x04, &t_reg2, 1);
        t_reg2 = 0x0E;
        i2c1Write(VI5300_DEVICE_ADDR, 0x05, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x08, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c1Write(VI5300_DEVICE_ADDR, 0x37, &t_reg2, 1);
        t_reg2 = 0x30;
        i2c1Write(VI5300_DEVICE_ADDR, 0x38, &t_reg2, 1);
        t_reg2 = 0x30;
        i2c1Write(VI5300_DEVICE_ADDR, 0x3A, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x39, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c1Write(VI5300_DEVICE_ADDR, 0x3B, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c1Write(VI5300_DEVICE_ADDR, 0x3C, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c1Write(VI5300_DEVICE_ADDR, 0x3D, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x3E, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c1Write(VI5300_DEVICE_ADDR, 0x3F, &t_reg2, 1);
        t_reg2 = 0x0E;
        i2c1Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x0F;
        i2c1Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);

        retry = 0;
        do
        {
            Ddl_Delay1ms(1);                              // delay 1ms
            i2c1Read(VI5300_DEVICE_ADDR, 0x02, &stat, 1); // stat = 0x10
        } while ((retry++ < 20) && (stat & 0x01));

        //        i2c1Read(VI5300_DEVICE_ADDR, 0x08, &stat, 1);
        //        Ddl_Delay1ms(40);
        //        do
        //        {
        //            t_reg2 = 0x00;
        //            i2c1Write(VI5300_DEVICE_ADDR, 0x08, &t_reg2, 1);
        //            Ddl_Delay1ms(1);
        //            i2c1Read(VI5300_DEVICE_ADDR, 0x08, &stat, 1);
        //        } while (stat != 0 && retry++ < 5);
        // 芯片固件开始启动，大约持续 40 ms，
        // 可以通过确认 0x08 寄存器是否为 0x66 来判断是否启动成功。
        // 固件启动后，芯片会自动进入软件休眠模式。
        retry = 0;
        do
        {
            VI5300_DownloadFirmware(g_pVi5300Firmware, getFirmwareLen());
            Ddl_Delay1ms(10); // delay 10ms
            i2c1Read(VI5300_DEVICE_ADDR, 0x08, &stat, 1);
        } while ((stat != 0x55) && (stat != 0x66) && (retry++ < 5));

        if (retry >= 5)
        {
            existenceFlag = 0;
            return existenceFlag;
        }

        existenceFlag = 1;
        VI5300_Interrupt_Enable();
        VI5300_Set_Integralcounts_Frame(30, 131072);
        /**添加的程序****/
        ReadCalibrationDataFromFile(&cali_data);
        if (cali_data.Cali_Flag == 0xFF)
        {
            cali_data.Cali_Flag = 0;
        }

#ifdef VI5300_TEMP_CALIBRATION
        if ((cali_data.Cali_Flag & 0xF0) != 0x10)
        {
            // 标定在第一次开机执行，或者工厂模式执行，后面不能执行
            VI5300_Start_RefTof_Calibration(&offset_data);
            cali_data.Cali_Flag |= 0x10; // RefTof&offset calibration flag
            cali_data.RefTof = offset_data.RefTof;
            cali_data.offset_cal = offset_data.offset_cal;
        }
        else
        {
            offset_data.RefTof = cali_data.RefTof;
            offset_data.offset_cal = cali_data.offset_cal;
        }

        VI5300_Config_RefTof(offset_data.RefTof);
#endif
        /*****/
        // 开始测量
        VI5300_Start_Continuous_Measure();
    }
    return existenceFlag;
}

void VI5300_GetRawRangingData(VI5300_Dist_TypeDef *result)
{
    uint8_t buf[32], i;
    int16_t milimeter;
    uint16_t noise, noise_level;
    uint32_t peak1;
    uint32_t peak2, peak1_r, noise_r;
    uint32_t integral_times;
    uint32_t peak_tmp;
    uint32_t lower;
    uint32_t upper;
    int32_t bias;
    uint32_t confidence;
    uint32_t s_noise[10] = {19, 83, 163, 691, 1243, 2539, 5091, 10395, 20427, 33235};
    uint32_t s_lower[10] = {16, 25, 45, 145, 258, 501, 1004, 1980, 3804, 6063};
    uint32_t s_upper[10] = {26, 35, 61, 173, 303, 574, 1148, 2220, 4230, 6705};

    i2c1Read(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, buf, 32);

    /***********add the function of pileup and confidence 20210105  小端模式*********/
    milimeter = (short)((((short)buf[13]) << 8) | (((short)buf[12])));
    peak1 = (uint32_t)((((uint32_t)buf[31]) << 24) | ((uint32_t)buf[30] << 16) | ((uint32_t)buf[29] << 8) | ((uint32_t)buf[28]));
    noise_level = (uint16_t)((((uint16_t)buf[27]) << 8) | (((uint16_t)buf[26])));
    peak2 = (uint32_t)((((uint32_t)buf[11]) << 24) | (((uint32_t)buf[10]) << 16) | (((uint32_t)buf[9]) << 8) | ((uint32_t)buf[8]));

    integral_times = (uint32_t)((((uint32_t)buf[25]) << 24) | (((uint32_t)buf[24]) << 16) | (((uint32_t)buf[23]) << 8) | ((uint32_t)buf[22]));
    integral_times = integral_times & 0x00ffffff;

    peak_tmp = peak2 * 16 / integral_times;
    bias = (int32_t)(PILEUP_A / (PILEUP_B - peak_tmp * PILEUP_D) - PILEUP_C) / PILEUP_D;
    if (bias < 0)
    {
        bias = 0;
    }
    milimeter = milimeter + (int16_t)bias;

#ifdef VI5300_OFFSET_CALIBRATION
    milimeter = milimeter - offset_data.offset_cal; // 减去offset
#endif
    ////update by 2022/03/07
    if (peak1 > 8000000)
        peak1_r = peak1 * 256 / integral_times * 4;
    else if (peak1 > 4000000)
        peak1_r = peak1 * 512 / integral_times * 2;
    else
        peak1_r = peak1 * 1024 / integral_times;

    noise_r = noise_level * 65536 / integral_times * 2;

    for (i = 0; i < 9; i++)
    {
        if (noise_r < s_noise[i + 1])
        {
            lower = (s_lower[i + 1] - s_lower[i]) * abs(noise_r - s_noise[i]) / (s_noise[i + 1] - s_noise[i]) + s_lower[i];
            upper = (s_upper[i + 1] - s_upper[i]) * abs(noise_r - s_noise[i]) / (s_noise[i + 1] - s_noise[i]) + s_upper[i];
            break;
        }
        else if (noise_r >= s_noise[9])
        {
            lower = (s_lower[9] - s_lower[8]) * abs(noise_r - s_noise[8]) / (s_noise[9] - s_noise[8]) + s_lower[8];
            upper = (s_upper[9] - s_upper[8]) * abs(noise_r - s_noise[8]) / (s_noise[9] - s_noise[8]) + s_upper[8];
            break;
        }
    }
    if (peak1_r < lower)
    {
        confidence = 0;
    }
    else if (peak1_r > upper)
    {
        confidence = 100;
    }
    else
    {
        confidence = 100 * (peak1_r - lower) / (upper - lower);
    }

    //		if(confidence >= 90)
    //		{
    //				result->status = STATUS_TOF_CONFIDENT;
    //		}
    //		else if(confidence >= 30)
    //		{
    //				result->status = STATUS_TOF_SEMI_CONFIDENT;
    //		}
    //		else
    //		{
    //				result->status = STATUS_TOF_NOT_CONFIDENT;
    //		}
    //		if(milimeter < 50 && peak1 < 800000)
    //		{
    //			//confidence =0;
    //				result->status = STATUS_TOF_NOT_CONFIDENT;
    //		}
    result->millimeter = milimeter;
    result->peak = peak1;
    result->noise = noise;
    result->confidence = confidence;
}

uint8_t VI5300_Get_Measure_Data(uint16_t *dist)
{
    uint8_t int_status = 0;
    i2c1Read(VI5300_DEVICE_ADDR, VI5300_RET_INT_STATUS, &int_status, 1);
    if ((int_status & 0x01) == 0x00)
    {
        return 0;
    }
    VI5300_GetRawRangingData(&distance);

    if (distance.confidence > 30 && distance.millimeter > 0)
    {
        *dist = distance.millimeter * 0.1; // dist单位为cm
    }
    else
    {
        return 0;
    }
    return 1;
}
